package it.unicam.cs.cosy.bioshape.core;

import it.unicam.cs.cosy.bioshape.core.components.ShapedEntity;

import javax.vecmath.Matrix3d;
import javax.vecmath.Vector3d;

import vclip.DistanceReport;
import vclip.Matrix4dX;
import vclip.PolyTree;

public class TestVClip {

	public static void main(String args[]) {
		// double[] points = new double[] { 2, 2, 2, 0, 2, 2, 0, 0, 2, 2, 0, 2,
		// 2,
		// 2, 0, 0, 2, 0, 0, 0, 0, 2, 0, 0 };
		//
		// int[][] faceIndices = { { 0, 1, 2, 3 }, { 0, 3, 7, 4 }, { 7, 6, 5, 4
		// },
		// { 6, 2, 1, 5 }, { 1, 0, 4, 5 }, { 3, 2, 6, 7 } };
		// //
		// //
		// //
		double[] points1 = new double[] { 10, 2, 2, 8, 2, 2, 8, 0, 2, 10, 0, 2,
				10, 2, 0, 8, 2, 0, 8, 0, 0, 10, 0, 0 };

		int[][] faceIndices1 = { { 0, 1, 2, 3 }, { 0, 3, 7, 4 },
				{ 7, 6, 5, 4 }, { 6, 2, 1, 5 }, { 1, 0, 4, 5 }, { 7, 6, 2, 3 } };

		// // cubo di lato DUE con centro in (1,1,1)
		// ConvexPolyhedron mioCubo = new ConvexPolyhedron(points, faceIndices);
		// // cubo di lato DUE con centro in (9,1,1)
		// ConvexPolyhedron mioCubo1 = new ConvexPolyhedron(points1,
		// faceIndices1);
		try {
			// PolyTree ptree1 = new PolyTree("PinoPinguino", mioCubo);
			// PolyTree ptree2 = new PolyTree("PinoPinguino", mioCubo1);
			// traslation for the first cube
			// PolyTree ptree1 = new PolyTree("PinoPinguino",
			// PolyTree.createBox(
			// "pippo", 2, 2, 2));
			// PolyTree ptree2 = new PolyTree("PinoPinguino2",
			// PolyTree.createBox(
			// "pippo2", 2, 2, 2));
			PolyTree ptree1 = PolyTree.createBox("pippo", 2, 2, 2);
			PolyTree ptree2 = PolyTree.createBox("pippo2", 2, 2, 2);
			PolyTree ptree3 = PolyTree.createBox("pippo3", 2, 2, 2);
			ptree3.getTransform().setTranslation(new Vector3d(9.00001, 2, 1));
			Vector3d vel = new Vector3d(5, 0, 0);
			Vector3d rottt = new Vector3d(Math.toRadians(0), Math.toRadians(0),
					Math.toRadians(0));
			Vector3d vel2 = new Vector3d(-5, 0, 0);
			Vector3d rottt2 = new Vector3d(Math.toRadians(0),
					Math.toRadians(0), Math.toRadians(0));
			//
			Matrix4dX rotateMe = new Matrix4dX();
			rotateMe.setRpy(Math.toRadians(0), Math.toRadians(0),
					Math.toRadians(0));
			rotateMe.setTranslation(new Vector3d(5.0, 2, 1));
			ptree1.setTransform(rotateMe);
			System.out.println(rotateMe);
			ptree2.getTransform().setTranslation(new Vector3d(7.00001, 2, 1));
			//
			// ***************************************************
			// FONDAMENTALI PER LA CORRETTA RESPONSE
			DistanceReport drep = new DistanceReport();
			drep.setFeaturePromotion(true, Math.toRadians(10));
			drep.setMaxPairDistance(Double.POSITIVE_INFINITY);
			drep.setMaxClosePairs(50);
			// ***************************************************
			// traslation from second polytree to first. Useless for our usages
			Matrix4dX X21 = new Matrix4dX(); // automatically identity
			System.out.println("TEST V-CLIP");
			double dist = ptree1.vclip(drep, ptree2, X21, -1, null);
			System.out.println("Distance: " + dist);
			System.out.println("\nFeatures: \""
					+ drep.getClosestPair().feat1.getName() + "\" AND \""
					+ drep.getClosestPair().feat2.getName()
					+ "\"\nCoordinates: " + drep.getClosestPair().pnt1
					+ " --* AND *-- " + drep.getClosestPair().pnt2);
			System.out.println("\nNormal: " + drep.getClosestPair().nrml);

			Matrix4dX trans = new Matrix4dX();
			trans.m03 = 2.002;
			trans.m13 = 0;
			trans.m23 = 0;
			ptree1.addComponent("", ptree2, trans);
			ptree1.buildBoundingHull(PolyTree.OBB_HULL);
			if (!ptree1.isAtomic())
				System.out.println(ptree1.volume());
			Vector3d v = new Vector3d();
			ptree1.centerOfMass(v);
			System.out.println(ptree1.getTransform());
			System.out.println(v);
			Matrix4dX trans2 = new Matrix4dX();
			trans2.setTranslation(new Vector3d(2, 2, 1));
			Matrix4dX trans3 = new Matrix4dX();
			trans3.setTranslation(new Vector3d(3, 2, 1));
			ptree1.setTransform(trans2);
			// ptree1.setTransform(trans3);
			drep = new DistanceReport();
			drep.setFeaturePromotion(true, Math.toRadians(10));
			drep.setMaxPairDistance(Double.POSITIVE_INFINITY);
			drep.setMaxClosePairs(50);
			dist = ptree1.vclip(drep, ptree3, X21, -1, null);
			System.out.println("Distance: " + dist);
			System.out.println("\nFeatures: \""
					+ drep.getClosestPair().feat1.getName() + "\" AND \""
					+ drep.getClosestPair().feat2.getName()
					+ "\"\nCoordinates: " + drep.getClosestPair().pnt1
					+ " --* AND *-- " + drep.getClosestPair().pnt2);
			System.out.println("\nNormal: " + drep.getClosestPair().nrml);
			// // data
			// double ma = 1;
			// double mb = 1;
			// //
			// // INERTIA TENSOR1 = rot I_1_body rotT
			// Matrix3d I1 = new Matrix3d();
			// Matrix3d rot = new Matrix3d();
			// ptree1.inertiaTensor(I1);
			// //
			// double ratio = ma / ptree1.volume();
			// I1.mul(ratio);
			// //
			// ptree1.getTransform().get(rot);
			// Matrix3d rotT = new Matrix3d(rot);
			// rotT.transpose();
			// I1.mul(rot, I1);
			// I1.mul(I1, rotT);
			// System.out.println("\n" + I1);
			//
			// // INERTIA TENSOR2 = rot I_2_body rotT
			// Matrix3d I2 = new Matrix3d();
			// Matrix3d rot2 = new Matrix3d();
			// ptree2.inertiaTensor(I2);
			// //
			// ratio = mb / ptree2.volume();
			// I2.mul(ratio);
			// //
			// ptree2.getTransform().get(rot2);
			// Matrix3d rotT2 = new Matrix3d(rot2);
			// rotT2.transpose();
			// I2.mul(rot2, I2);
			// I2.mul(I2, rotT2);
			// System.out.println(I2);
			// // SUPPOSITIONS:
			// // PTREE1 - mass 1, velocity above
			// // PTREE2 - mass inf velocity 0
			// //
			// // coefficient = 2 elastic, 1 inelastic!
			// double e = 1;
			// //
			// // contact point offset...
			// // OFFSET 1
			// Vector3d center1 = new Vector3d();
			// ptree1.centerOfMass(center1);
			// center1.x += ptree1.getTransform().m03;
			// center1.y += ptree1.getTransform().m13;
			// center1.z += ptree1.getTransform().m23;
			// //
			// Vector3d r1 = new Vector3d(drep.getClosestPair().pnt1);
			// r1.sub(center1);
			// // OFFSET 2
			// Vector3d center2 = new Vector3d();
			// ptree2.centerOfMass(center2);
			// center2.x += ptree2.getTransform().m03;
			// center2.y += ptree2.getTransform().m13;
			// center2.z += ptree2.getTransform().m23;
			// //
			// Vector3d r2 = new Vector3d(drep.getClosestPair().pnt2);
			// r2.sub(center2);
			// // normal
			// Vector3d nrm = new Vector3d();
			// nrm.normalize(drep.getClosestPair().nrml);
			// //
			// // Denominator for 1
			// I1.invert();
			// Vector3d angularVelChangea = new Vector3d();
			// angularVelChangea.cross(r1, nrm);
			// I1.transform(angularVelChangea);
			// Vector3d vaLinDueToR = new Vector3d();
			// vaLinDueToR.cross(angularVelChangea, r1);
			// // Denominator for 2
			// I2.invert();
			// Vector3d angularVelChangeb = new Vector3d();
			// angularVelChangeb.cross(r2, nrm);
			// I2.transform(angularVelChangeb);
			// Vector3d vbLinDueToR = new Vector3d();
			// vbLinDueToR.cross(angularVelChangeb, r2);
			// //
			// // final denominator
			// double scalar = (1 / mb) + (1 / ma) + vaLinDueToR.dot(nrm)
			// + vbLinDueToR.dot(nrm);
			// // calculate numerator
			// Vector3d om = new Vector3d();
			// om.cross(rottt, r1);
			// Vector3d velCopy1 = new Vector3d(vel);
			// velCopy1.add(om);
			// om.set(0, 0, 0);
			// om.cross(rottt2, r2);
			// Vector3d velCopy2 = new Vector3d(vel2);
			// velCopy2.add(om);
			// //
			// Vector3d vell = new Vector3d();
			// vell.sub(velCopy2, velCopy1);
			// double Jmod = (-(e + 1) * vell.dot(nrm)) / scalar;
			// System.out.println("Numerator: " + (-(e + 1) * vell.dot(nrm))
			// + "\nDenominator: " + scalar + "\nResult: " + Jmod + "\n");
			// //
			// Vector3d ja = new Vector3d(nrm);
			// Vector3d jb = new Vector3d(nrm);
			// ja.scale(Jmod / ma);
			// jb.scale(Jmod / mb);
			// System.out.println("t1+++  " + vel + "  +++");
			// System.out.println("t2+++  " + vel2 + "  +++");
			// System.out.println("r1+++  " + rottt + "  +++");
			// System.out.println("r2+++  " + rottt2 + "  +++");
			// vel.sub(ja);
			// vel2.add(jb);
			// rottt.sub(angularVelChangea);
			// rottt2.sub(angularVelChangeb);
			// System.out.println("t1+++  " + vel + "  +++");
			// System.out.println("t2+++  " + vel2 + "  +++");
			// System.out.println("r1+++  " + rottt + "  +++");
			// System.out.println("r2+++  " + rottt2 + "  +++");
		} catch (Exception e) {
			e.printStackTrace();
			System.exit(1);
		}
		// microBench();
	}

	private static void microBench() {
		long start = System.currentTimeMillis();
		maxAbsIndex(21.4234332, -3.2343242, -5.2324342);
		System.out.println(System.currentTimeMillis() - start);
		start = System.currentTimeMillis();
		maxAbsIndex2(21.4234332, -3.2343242, -5.2324342);
		System.out.println(System.currentTimeMillis() - start);
	}

	static int maxAbsIndex(double x, double y, double z) {
		double absx = (x < 0 ? -x : x);
		double absy = (y < 0 ? -y : y);
		double absz = (z < 0 ? -z : z);

		if (absx >= absy)
			return (absx >= absz) ? 0 : 2;
		return (absy >= absz) ? 1 : 2;
	}

	static int maxAbsIndex2(double x, double y, double z) {
		if (Math.abs(x) >= Math.abs(y))
			return (Math.abs(x) >= Math.abs(z)) ? 0 : 2;
		return (Math.abs(y) >= Math.abs(z)) ? 1 : 2;
	}

	private void elasticResponseWithRotation(ShapedEntity e1, ShapedEntity e2,
			DistanceReport drep) {
		// INERTIA TENSOR1 = rot I_1_body rotT
		Matrix3d I1 = new Matrix3d();
		Matrix3d rot = new Matrix3d();
		e1.inertiaTensor(I1);
		e1.getTransform().get(rot);
		Matrix3d rotT = new Matrix3d(rot);
		rotT.transpose();
		I1.mul(rot, I1);
		I1.mul(I1, rotT);
		//
		// INERTIA TENSOR2 = rot I_2_body rotT
		Matrix3d I2 = new Matrix3d();
		Matrix3d rot2 = new Matrix3d();
		e2.inertiaTensor(I2);
		e2.getTransform().get(rot2);
		Matrix3d rotT2 = new Matrix3d(rot2);
		rotT.transpose();
		I2.mul(rot2, I2);
		I2.mul(I2, rotT2);
		// DATA
		double ma = e1.getMass();
		double mb = e2.getMass();
		Vector3d vel1 = e1.getInstantVelocity();
		Vector3d vel2 = e2.getInstantVelocity();
		Vector3d rotA = e1.getInstantAngularVelocity();
		Vector3d rotB = e1.getInstantAngularVelocity();
		// OFFSET for A
		Vector3d center1 = e1.getCenter();
		Vector3d ra = new Vector3d(drep.getClosestPair().pnt1);
		ra.sub(center1);
		// OFFSET for B
		Vector3d center2 = e2.getCenter();
		Vector3d rb = new Vector3d(drep.getClosestPair().pnt2);
		rb.sub(center2);
		//
		// norm to the collision plane
		Vector3d nrm = new Vector3d();
		nrm.normalize(drep.getClosestPair().nrml);
		//
		// calculate impulse denominator (for A)
		I1.invert();
		Vector3d angularVelChangea = new Vector3d();
		angularVelChangea.cross(ra, nrm);
		I1.transform(angularVelChangea);
		Vector3d vaLinDueToR = new Vector3d();
		vaLinDueToR.cross(angularVelChangea, ra);
		// calculate impulse denominator (for B)
		I2.invert();
		Vector3d angularVelChangeb = new Vector3d();
		angularVelChangeb.cross(rb, nrm);
		I2.transform(angularVelChangeb);
		Vector3d vbLinDueToR = new Vector3d();
		vbLinDueToR.cross(angularVelChangeb, rb);
		// full denominator of impulse J
		double scalar = 1 / mb + 1 / ma + vaLinDueToR.dot(nrm)
				+ vbLinDueToR.dot(nrm);
		//
		// calculate impulse numerator
		Vector3d vell = new Vector3d();
		Vector3d velCopy1 = new Vector3d(vel1);
		Vector3d velCopy2 = new Vector3d(vel2);
		Vector3d om = new Vector3d();
		om.cross(rotA, ra);

		velCopy1.add(om);
		om.set(0, 0, 0);
		om.cross(rotB, rb);
		velCopy2.add(om);
		vell.sub(velCopy2, velCopy1);
		double Jmod = (-2 * vell.dot(nrm)) / scalar;
		Vector3d J = new Vector3d(nrm);
		J.scale(Jmod);
		//
		Vector3d ja = new Vector3d(J);
		Vector3d jb = new Vector3d(J);
		ja.scale(1 / ma);
		jb.scale(1 / mb);
		// calculate final translation velocities
		vel1.sub(ja);
		vel2.add(jb);
		e1.setInstantVelocity(vel1);
		e2.setInstantVelocity(vel2);
		// calculate final rotation velocities
		angularVelChangea.scale(Jmod);
		angularVelChangeb.scale(Jmod);
		rotA.sub(angularVelChangea);
		rotB.add(angularVelChangeb);
		e1.setInstantAngularVelocity(rotA);
		e2.setInstantAngularVelocity(rotB);
	}
}
